
#ifndef __MLS_BLENSKELMESHDEFORMER_H__
#define __MLS_BLENSKELMESHDEFORMER_H__

#include "../core/tconstraints.h"
#include "mlsSkelMeshDeformer.h"
#include "gskeleton.h"

template <class T>
class MlsBlenSkelMeshDeformer :public MlsSkelMeshDeformer<T> {
   
   typedef TConstraints<Point3>     PConstraints;
   typedef TConstraints<Segment3>   SConstraints;
public:
   virtual ~MlsBlenSkelMeshDeformer() {
   }
   
   /// Constructor of a mesh deformer driven by a skeleton
   /// @param m the mesh model
   /// @param constraints the initial constraints
   MlsBlenSkelMeshDeformer(GSkeleton *s, const T &constraints, Mesh *m) {
      // copy the initial positions
      assert(m != 0);
      assert(s != 0);
      skeletonIni = *s;
      this->skeleton = s;
      this->model = m;
      this->ipositions.resize(this->model->nvertices());
      this->inormals.resize(this->model->nvertices());
      Vertex_iterator vi = this->model->vertices_begin();
      for (unsigned int i=0; i<this->ipositions.size(); ++i, ++vi) {
         this->ipositions[i] = vi->point();
         this->inormals[i] = vi->normal();
      }
      
      this->isGeodesic = true;
      this->cosourcE = constraints;
      this->coSource = &this->cosourcE;
      //~ this->coTarget = &this->cotarget;
      
//      this->skeleton->kdist[0] = 0;
//      this->skeleton->kdist[1] = 0;
//      this->skeleton->kdist[2] = 0;
//      this->skeleton->kdist[3] = 0;
//      this->skeleton->kdist[4] = 0;
//      this->skeleton->kdist[5] = 0;
//      this->skeleton->kdist[6] = 0;
//      this->skeleton->kdist[7] = 0;
//      this->skeleton->kdist[8] = 0;
//      this->skeleton->kdist[9] = 0;
//      this->skeleton->kdist[10] = 0;
//      this->skeleton->kdist[11] = 0;
//      this->skeleton->kdist[12] = 0;
//      this->skeleton->kdist[13] = 0;
//      this->skeleton->kdist[14] = 0;
//      this->skeleton->kdist[15] = 0;
//      this->skeleton->kdist[16] = 0;
//      this->skeleton->kdist[17] = 0;
//      this->skeleton->kdist[18] = 0;
      
      /// first case:
//      this->skeleton->kdist[0] = 0;
//      this->skeleton->kdist[1] = 0;
//      this->skeleton->kdist[2] = 0;
//      this->skeleton->kdist[3] = 0;
//      this->skeleton->kdist[4] = 0;
//      this->skeleton->kdist[5] = 500;
//      this->skeleton->kdist[6] = 1500;
//      this->skeleton->kdist[7] = 1000;
//      this->skeleton->kdist[8] = 2500;
//      this->skeleton->kdist[9] = 0;
//      this->skeleton->kdist[10] = 1000;
//      this->skeleton->kdist[11] = 0;
//      this->skeleton->kdist[12] = 500;
//      this->skeleton->kdist[13] = 0;
//      this->skeleton->kdist[14] = 0;
//      this->skeleton->kdist[15] = 0;
//      this->skeleton->kdist[16] = 0;
//      this->skeleton->kdist[17] = 0;
//      this->skeleton->kdist[18] = 2000;
      


      /// second case: de frente 
//      this->skeleton->kdist[0] = 0;
//      this->skeleton->kdist[1] = 0;
//      this->skeleton->kdist[2] = 0;
//      this->skeleton->kdist[3] = 0;
//      this->skeleton->kdist[4] = 0;
//      this->skeleton->kdist[5] = 0;
//      this->skeleton->kdist[6] = 0;
//      this->skeleton->kdist[7] = 0;
//      this->skeleton->kdist[8] = 4000;
//      this->skeleton->kdist[9] = 3000;
//      this->skeleton->kdist[10] = 0;
//      this->skeleton->kdist[11] = 0;
//      this->skeleton->kdist[12] = 0;
//      this->skeleton->kdist[13] = 0;
//      this->skeleton->kdist[14] = 0;
//      this->skeleton->kdist[15] = 0;
//      this->skeleton->kdist[16] = 0;
//      this->skeleton->kdist[17] = 0;
//      this->skeleton->kdist[18] = 5000;

      //
      genDistanceTable(this->coSource);
      
      // generate the table of weights
      this->genWeightTable(this->coSource);
      //cout<<"genWeightTable: ... "<<endl;
      //for (int i=0; i<this->weightTable.size(); ++i) {
         //cout<<endl;
         //for (int j=0; j<this->weightTable.size(); ++j) {
            //cout<<this->weight(i,j)<<" ";
         //}
      //}
      
      genWeightTable1(this->coSource);

      pstarl.resize(this->skeleton->nJoints(), Vector3(0,0,0));
      qstarl.resize(this->skeleton->nJoints(), Vector3(0,0,0));
      ul.resize(this->skeleton->nJoints(), Vector3(0,0,0));
   }
   
   virtual void execute(const T &constraintsTarget) {
      
      // compute the skeleton deformation
      this->coTarget = &constraintsTarget;
      //~ cout<<" ... !execute begin .... : "<<this->coSource->size()<<" : "<<this->coTarget->size()<<endl;
      for (int i=0; i<this->skeleton->nJoints(); ++i) {
         this->pstar = computeCentroid(i, this->coSource);
         /// classic situation
         this->qstar = computeCentroid(i, this->coTarget);

         /// new situation
         Number x=0, y=0, z=0, ws=0;
         for (uint j=0; j<this->coTarget->size(); ++j) {
            const Number &w = this->weight1(i, j);
            const Point3 &p = this->coSource->getConstraint(j);
            const Point3 &q = this->coTarget->getConstraint(j);
            ws += w;
            x += w*(q[0]-p[0]);
            y += w*(q[1]-p[1]);
            z += w*(q[2]-p[2]);
         }
         //~ cout<<"p<< "<<this->pstar<<endl;
         //~ cout<<"q<< "<<this->qstar<<endl;
         //~ cout<<"<< "<<x/ws<<" "<<y/ws<<" "<<z/ws<<endl;
         this->qstar = Point3(x/ws + this->pstar[0], y/ws + this->pstar[1], z/ws + this->pstar[2]);
//         this->qstar = Point3(x/ws, y/ws, z/ws);
         //~ cout<<"Q>> "<<this->qstar<<endl;
         
         computeCorrelationMatrix(i, this->coSource, this->coTarget);
         Number maxroot = this->computeLargestRootOfQuarticEquation();
         if (isnan(maxroot)) {
            this->u[0] = this->u[1] = this->u[2] = 0.0;
         }
         else {
            this->eigenvalue = maxroot + this->E;
            this->computeRotationVector(); // computes the u value
         }
         
         //cout<<"  "<<maxroot<<" : "<<this->eigenvalue<<endl;
         //cout<<"  "<<this->u[0]<<" "<<this->u[1]<<" "<<this->u[2]<<endl;
         
//         Point3 p = this->skeleton->getJointPos(i);
         Point3 p = skeletonIni.getJointPos(i);
         Vector3 phat = p - this->pstar;
         Vector3 vr = this->rotate(phat);
         this->skeleton->setJointPos(i, this->qstar + vr);
         
         pstarl[i] = this->pstar - Point3(0,0,0);
         qstarl[i] = this->qstar - Point3(0,0,0);
         ul[i] = Vector3(this->u[0], this->u[1], this->u[2]);
         
         //cout<<"   "<<ul[i]<<endl;
         //~ cout<<"   "<<pstarl[i]<<endl;
         //~ cout<<"   "<<qstarl[i]<<endl;
      }
      
      // project deformation on the mesh
      int i=0;
      Vertex_iterator vi = this->model->vertices_begin();
      while (vi != this->model->vertices_end()) {
         Vector3 _pstar(0,0,0), _qstar(0,0,0); 
         this->u[0]=this->u[1]=this->u[2]=0.0;
         for (uint j=0; j<vi->weights.size(); ++j) {
            this->u[0] += vi->weights[j]*ul[j][0];
            this->u[1] += vi->weights[j]*ul[j][1];
            this->u[2] += vi->weights[j]*ul[j][2];
            _pstar = _pstar + vi->weights[j]*pstarl[j];
            _qstar = _qstar + vi->weights[j]*qstarl[j];
         }
         
         //// with rotation
         Point3 p = this->ipositions[i];
         Vector3 phat(p[0]-_pstar[0], p[1]-_pstar[1], p[2]-_pstar[2]);
         Vector3 vr = this->rotate(phat);
         vi->point() = Point3(_qstar[0]+vr[0], _qstar[1]+vr[1], _qstar[2]+vr[2]); // qstar + vr
         
         //~ cout<<":: "<<i<<" : "<<p<<" :: "<<this->u[0]<<" "<<this->u[1]<<" "<<this->u[2]<<endl;
         //~ cout<<"   "<<_pstar<<" : "<<_qstar<<" : "<<phat<<" : "<<vr<<endl;
         
         /// without rotation
         //~ Point3 t = vi->point()-_pstar;
         //~ vi->point() = t + _qstar;
         
         vi->normal() = this->rotate(this->inormals[i]);
         
         ++vi; ++i;
      }
      
      cout<<" ... !execute done .... "<<endl;
   }
   
private:
   
   virtual void genDistanceTable(const PConstraints *constraints) {
      
      cout<<" ... computing the distance table (PConstraints) mlsblendskelmeshdeformer... "<<endl;
      const int &njoints = this->skeleton->nJoints();
      this->distanceTable.resize(njoints);
      distanceTable1.resize(njoints);
      for (uint i=0; i<this->distanceTable.size(); ++i) {
         this->distanceTable[i].resize(constraints->size(),0.0);
         distanceTable1[i].resize(constraints->size(),0.0);
      }
      
      bool isPossible = true; /// its possible to compute a geodesic metric over the structure?
      //if (this->model->isClustered) isPossible = true;
      
      if (isPossible and this->isGeodesic) {
         cout<<" ... computing vertex/joint distances walking over the skeleton... "<<endl;
         //~ for (int i=0; i<njoints; ++i) {
            //~ this->skeleton->floodfill_Dijkstra(i);
            //~ for (int j=0; j<constraints->size(); ++j) {
               //~ this->distanceTable[i][j] = this->skeleton->d[j];
            //~ }
         //~ }
         for (uint j=0; j<constraints->size(); ++j) {
            cout<<"j: "<<j<<endl;
            this->skeleton->floodfill_Dijkstra(constraints->ids[j], false);
            for (int i=0; i<njoints; ++i) {
               this->distanceTable[i][j] = this->skeleton->d[i];
            }
            this->skeleton->floodfill_Dijkstra(constraints->ids[j], true);
            for (int i=0; i<njoints; ++i) {
               this->distanceTable1[i][j] = this->skeleton->d[i];
            }
         }
      } else {
         cout<<" ... computing euclidian distances ... "<<endl;
         for (uint i=0; i<this->model->nvertices(); ++i) {
            for (uint j=0; j<this->coSource->size(); ++j) {
               const Point3 &P = this->ipositions[i];
               const Point3 &p = constraints->getConstraint(j);
               Vector3 v(p,P);
               this->distanceTable[i][j] = sqrt(v*v);
            }
         }
      }
      cout<<" ... computing the distance table ... done"<<endl;
   }
   
   /// Generates the table of weights. Each entry [i][j] maps the weight 
   /// corresponding between the joint i and the constraint j
   void genWeightTable1(const PConstraints *constraints) {
      
      assert(constraints != 0);
      assert(not this->distanceTable1.empty());
      
      cout<<" ... computing the weights1 table .... "<<endl;
      weightTable1.resize(this->distanceTable1.size());
      for (uint i=0; i<weightTable1.size(); ++i) {
         const uint &n = distanceTable1[i].size();
         weightTable1[i].resize(n, 0.0);
         for (uint j=0; j<n; ++j) {
            weightTable1[i][j] = this->omega(distance1(i, j));
         }
      }
      cout<<" ... computing the weights table .... done "<<endl;
   }
   
   const Number& weight1(int i, int j) const {
      return weightTable1[i][j];
   }
   
   const Number& distance1(int i, int j) const {
      return distanceTable1[i][j];
   }
   
public:
   
   vector<Vector3>            pstarl;
   vector<Vector3>            qstarl;
   vector<Vector3>            Qstarl;
   vector<Vector3>            ul;

   GSkeleton                  skeletonIni; // the initial skeleton   

   // bending control structures
   vector<vector<Number> >    distanceTable1;
   vector<vector<Number> >    weightTable1;
};

#endif
